#include "align_enu/align_enu.h"
#include "ceres/ceres.h"
#include "align_enu/align_error_term.hpp"
align_ENU::align_ENU(/* args */)
{
    q.setIdentity();
    t.setZero();
    origin_point_size = 0;
    align_to_point_size = 0;
}

align_ENU::~align_ENU()
{
}
void align_ENU::add_origin_path_point(Eigen::Vector3d pose,double time_stamp){
    PathPointType ppt;
    ppt.point = pose;
    ppt.time_stamp = time_stamp;
    ppt.point_type = 1;
    point_seq.push_back(ppt);
    origin_point_size++;
}
void align_ENU::add_align_to_path_point(Eigen::Vector3d pose,double time_stamp){
    PathPointType ppt;
    ppt.point = pose;
    ppt.time_stamp = time_stamp;
    ppt.point_type = 0;
    point_seq.push_back(ppt);
    align_to_point_size++;
}
bool align_ENU::optimization(double &score){
    if (point_seq.size()<20)
    {
        return false;
    }

    std::sort(point_seq.begin(),point_seq.end(),[this](PathPointType&a,PathPointType&b){
                            return a.time_stamp<b.time_stamp;});
    ceres::LossFunction *loss_function = nullptr;
    ceres::Manifold *quaternion_manifold = new ceres::EigenQuaternionManifold;
    ceres::Problem *problem = new ceres::Problem();
    int cnt =1;
    for (int i = 1; i < point_seq.size()-1; i++)
    {
        if (point_seq[i].point_type==0&&point_seq[i-1].point_type==1&&point_seq[i+1].point_type==1)
        {
            cnt++;
            ErrorRelationType ert;
            ert.origin_point = point_seq[i].point;
            ert.align_to_front_point = point_seq[i-1].point;
            ert.align_to_back_point = point_seq[i+1].point;
            ceres::CostFunction *cost_function = AlignErrorTerm::Create(ert);
            problem->AddResidualBlock(cost_function,
                                    loss_function,
                                    t.data(),
                                    q.coeffs().data());
            problem->SetManifold(q.coeffs().data(),quaternion_manifold);
        }
    }
    //std::cout<<"size : "<<cnt-1<<std::endl;
    ceres::Solver::Options options;
    options.max_num_iterations= 300;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    ceres::Solver::Summary summary;
    ceres::Solve(options,problem,&summary);
    std::cout<<summary.BriefReport()<<"\n";
    score = summary.final_cost/cnt;

    return summary.IsSolutionUsable();
}
void align_ENU::get_result(Eigen::Quaterniond &q_,Eigen::Vector3d &t_){
    q_ = q;
    t_ = t;
}
void align_ENU::set_init_qt(Eigen::Quaterniond q_,Eigen::Vector3d t_){
    q = q_;
    t = t_;
}
int align_ENU::get_origin_point_size(){
    return origin_point_size;
}
int align_ENU::get_align_to_point_size(){
    return align_to_point_size;
}